#include "vrpn_server.hpp"
#include "animation.hpp"
#include "utils.h"

#include <memory>
#include <iostream>
#include <thread>
#include <string>
#include <algorithm>

const std::string usage = "\
usage:\n\
vrpn_server.exe\n\
\t--bvh=\"filePath\" \n\
\t--mocap-ip=x.x.x.x \n\
\t--mocap-port=int \n\
\t--vrpn-port=int \n\
\t--subject=\"name\" \n\
\t--euler-order=yxz \n\
\t--skip-tpose=1 \n\
\t--bone-prefix=\"\" \n\
"\
;

void print_usage ();
void get_str_param_value (const std::string& param_value, std::string& val);

int main(int argc, char** argv)
{
	try {
		// 1 + [required arguments' amount]
		if (argc < 1 + 3) {
			print_usage ();
			return -1;
		}

		std::string bvh_file;
		std::string subject_name;
		std::string mocap_ip;
		int mocap_port;
		int vrpn_port = 3883;
		EulerOrder euler_order = YXZ;
		int skip_tpose = 1;
		std::string bone_prefix = "";

		for (int i = 1; i < argc; i++) {
			string_vector param (std::move (split_string (argv[i], '=')));
			if ((param.size () != 2) ||
				(param[0].size() < 3)) {
				print_usage ();
				return -1;
			}

			// param_name & param_value
			std::string pn = param[0].substr (2);
			std::string pv = param[1];

			if (pn == "bvh") {
				if (pv.size () < 3) {
					print_usage ();
					return -1;
				}
				get_str_param_value (pv, bvh_file);
			}
			else if (pn == "mocap-ip") {
				if (pv.size () < 7) {
					print_usage ();
					return -1;
				}
				get_str_param_value (pv, mocap_ip);
			}
			else if (pn == "mocap-port") {
				std::string port;
				get_str_param_value (pv, port);
				mocap_port = atoi (port.c_str ());
			}
			else if (pn == "vrpn-port") {
				std::string port;
				get_str_param_value (pv, port);
				vrpn_port = atoi (port.c_str ());
			}
			else if (pn == "subject") {
				get_str_param_value (pv, subject_name);
			}
			else if (pn == "euler-order") {
				std::string order;
				get_str_param_value (pv, order);
				std::transform (order.begin (), order.end (), order.begin (), ::tolower);
				if (order == "xyz") euler_order = XYZ;
				else if (order == "xzy") euler_order = XZY;
				else if (order == "yxz") euler_order = YXZ;
				else if (order == "yzx") euler_order = YZX;
				else if (order == "zxy") euler_order = ZXY;
				else if (order == "zyx") euler_order = ZYX;
				else {}
				std::cout << "Mocap euler rotation order is " << order << " ....\n";
			}
			else if (pn == "skip-tpose") {
				std::string is_skip;
				get_str_param_value (pv, is_skip);
				skip_tpose = atoi (is_skip.c_str ());
			}
			else if (pn == "bone-prefix") {
				get_str_param_value (pv, bone_prefix);
			}
			else {
				print_usage ();
				return -1;
			}
		}

		std::cout << "reading bvh file " << bvh_file << "....\n";
		IKinema::Bvh bvh{ bone_prefix, bvh_file };
		bvh.eulerOrder() = euler_order;
		bvh.subject () = subject_name;
		if (bone_prefix.empty ()) {
			std::cout << "there is no bone prefix...\n";
		}
		else {
			std::cout << "bone prefix is " << bone_prefix << std::endl;
		}

		std::cout << "creating server on port " << vrpn_port << " ....\n";
		auto connection = vrpn_create_server_connection (vrpn_port);

		std::cout << "adding subject " << subject_name << " to server....\n";
		auto tracker = std::make_unique<IKinema::vrpn::SkeletonTracker>(connection, subject_name);

		// the SkeletonTracker needs to know about its skeleton before being able to stream it
		tracker->build_metadata (bvh.get_skeleton_description ());

		if (!skip_tpose) {
			std::cout << "streaming T-Pose...\n";
			bool should_exit = false;
			std::thread server_thread ([&]() {
				auto wait_until = std::chrono::system_clock::now ();
				while (!should_exit) {
					auto&& animation_frame = bvh.get_animation_frame ();
					tracker->send_frame (animation_frame); // use the tracker to send the frame to the connection

														   // serve frames at  30 fps
														   // using sleep_until would cancel out the delay introduced by send_frame
					wait_until += std::chrono::milliseconds (33);
					std::this_thread::sleep_until (wait_until);
				}
			});
			std::cout << "press enter to continue ...\n";
			std::cin.get ();	// wait for user input

			should_exit = true;

			server_thread.join (); // wait for the thread to finish
			std::cout << "stop T-Pose streaming....\n";
		}


		bvh.startServiceAt (tracker.get(), mocap_port);
		std::cout << "streaming animation....\n";

		std::cout << "press enter to exit....\n";
		std::cin.get (); // wait for user input

		bvh.stopService ();
		std::cout << "stopping server....\n";
	}
	catch (const std::exception& e) {
		std::cerr << "error: " << e.what() << std::endl;

		std::cout << "press enter to exit....\n";
		std::cin.get (); // wait for user input
	}

	return 0;
}

void print_usage () {
	std::cout << usage;
	std::cout << "press enter to exit....\n";
	std::cin.get (); // wait for user input
}

void get_str_param_value (const std::string& param_value, std::string& value) {
	if (param_value[0] == '\"') {
		value = param_value.substr (1, param_value.size () - 2);
	}
	else {
		value = param_value;
	}
}
